# define HOMO_OCP_USE_MA97
# define EIGEN_USE_BLAS

# include <homo_ocp.h>

# include "rtr_ST55_3D_C.h"
# include "cpp_tools.h"
# include "udpSettings.h"
# include "dataStruct.h"
# include "filePathSettings.h"

# define inf std::numeric_limits<double>::infinity()

int main(int argc, char const *argv[])
{
    RC_udp::readXML();
    
    cpp_tools::udpVecServer<int64_t> udp2lower(udpSettings::PORT::lower_solver::order_len, udpSettings::PORT::lower_solver::order, 1);

    // 生成问题
    homo_ocp::prob m_prob;
    HOMO_OCP_PROB(m_prob,rtr_ST55_3D);

    auto tmp = cpp_tools::csvRead(host_path + settings_path + "/mpc/csvProbData.csv");
    Eigen::VectorXd r_0 = tmp.leftCols(1);
    Eigen::VectorXd lbr = tmp.middleCols(1,1);
    Eigen::VectorXd ubr = tmp.rightCols(1);

    m_prob.lbx = lbr;
    m_prob.ubx = ubr;
    
    std::cout << "mpc solver online" << std::endl;
    
    // 记录
    # ifdef RECORD
    cpp_tools::csvRecorder p_Recorder(host_path + data_path + "/solver/p.csv");
    cpp_tools::csvRecorder sol_x_Recorder(host_path + data_path + "/solver/sol_x.csv");
    cpp_tools::csvRecorder time_Recorder(host_path + data_path + "/solver/time.csv");
    cpp_tools::csvRecorder s_Recoder(host_path + data_path + "/solver/s.csv"); // position中的s
    # endif

    // 建立求解器
    cpp_tools::mtDataManager<homo_ocp::solver> m_solver(m_prob,inf,4,1);
    {
        auto ac = m_solver.write();
        ac.access()->m_basicParam.iter_max = 20;
        ac.access()->m_basicParam.accept_tol = 1e-4;
        ac.access()->m_homotopy.iter_max = 20;
        ac.access()->m_homotopy.diff_lota_min = 1;
        ac.access()->m_barrier.m_updateMode = homo_ocp::barrier::updateMode::Monotone;
        ac.access()->m_basicParam.useHomotopy = true;
        ac.access()->m_barrier.mu_0 = 0.1;

        ac.access()->reLoadBoundary(&lbr, &ubr,1);
    }

    // 开始循环
    bool isRunning = true;
    cpp_tools::threadPool m_pool;
    cpp_tools::clock m_clock;
    double t_update = -1;

    std::thread t1([&]() -> void{
        while (isRunning)
        {
            if (udp2lower.receive() >= -1)
            {
                auto order = udp2lower.get();
    
                if (order(0) == static_cast<int64_t>(data::communicate::lower_solver::mpc_order::refresh)) // 刷新
                {
                    std::cout << "刷新" << std::endl;
    
                    m_solver.write().access()->clearWsInfo();
                    m_solver.write().access()->refresh();
    
                    udp2lower.sendBack(order);
                }
                else if (order(0) == static_cast<int64_t>(data::communicate::lower_solver::mpc_order::solve)) // 求解器
                {
                    if (t_update != -1 && m_clock.toc(0) - t_update >= 0.5e3) // 如果是长时间没有进行计算，我们认为温启动信息失效，改用冷启动
                    {
                        m_solver.write().access()->clearWsInfo();
                    }

                    Eigen::VectorXd p = order.middleRows(1,m_prob.n_p).cast<double>()/udpSettings::intScale;
                    Eigen::VectorXd s = order.middleRows(m_prob.n_p + 1,1).cast<double>()/udpSettings::intScale;
    
                    auto ac = m_solver.write();
                    ac.access()->reLoadParam(p);
    
                    homo_ocp::sol sol;
                    try
                    {
                        sol = ac.access()->solve(r_0);
                    }
                    catch(const int)
                    {
                        std::cout << "出现了意料之外的错误，求解器崩溃了" << std::endl;
    
                        ac.access()->clearWsInfo();
    
                        sol.x = Eigen::VectorXd::Constant(m_prob.n_x,std::numeric_limits<double>::quiet_NaN());
                    }
                    
                    Eigen::Vector<int64_t, Eigen::Dynamic> message = (sol.x*udpSettings::intScale).cast<int64_t>();
                    udp2lower.sendBack(message);
                    
                    # ifdef RECORD
                    p_Recorder.record(p);
                    s_Recoder.record(s);
                    time_Recorder.record(Eigen::VectorXd::Constant(1,cpp_tools::getCurrentTimeAsDouble()));
                    sol_x_Recorder.record(sol.x);
                    # endif

                    t_update = m_clock.toc(0);
                }
            }

            std::this_thread::sleep_for(std::chrono::microseconds(100)); // 每0.1ms搜索一次
        }
    });
    m_pool.push_back(t1);

    std::thread t2([&]() -> void{
        std::string str;
        while (isRunning)
        {
            std::getline(std::cin, str);
            if (str == "q")
            {
                isRunning = false;
            }
            else
            {
                std::cout << "请输入 \'q\' 进行保存和结束程序" << std::endl;
            }
        }
    });
    m_pool.push_back(t2);

    m_pool.joinAll();
    
    return 0;
}
